
#ifndef IEDVISION_H_
#define IEDVISION_H_

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/core/core.hpp>
#include <ied_vision/Localization.h>

#define DEBUG1
#define DEBUG2

#define COLOR_CHANNELS 3

namespace enc = sensor_msgs::image_encodings;

#ifdef DEBUG1
//We only want to display locally in debug mode
static const char WINDOW1[] = "Color Edge Mask";
#endif
#ifdef DEBUG2
static const char WINDOW2[] = "Range Edge Mask";
static const char WINDOW2A[] = "Range Edges";
#endif

class IEDVision
{
private:
	ros::NodeHandle handle;
	image_transport::ImageTransport transport;
	image_transport::Subscriber color_sub;
	image_transport::Subscriber depth_sub;
	ros::Subscriber pose_sub;
	ros::Publisher ied_pub;

	bool updated[2];
	cv::Mat rangeImage;
	cv::Mat rangeEdges;
	cv::Mat colorEdges[COLOR_CHANNELS];
	
	Localization_Env_Read gridParams;
	Localization iedLocations;
	
	static double colorBound[];
	static double rangeBound;
	static double detectionThreshold;
	static double cameraFieldOfView;
	
public:
	IEDVision();

	~IEDVision();

	void imageCb(const sensor_msgs::ImageConstPtr& msg);
	void depthCb(const sensor_msgs::ImageConstPtr& msg);
	void poseCb(const geometry_msgs::PoseStampedConstPtr& msg);
	void findIEDs(void);
};

#endif //IEDVISION_H